// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PlanarPoseEstimationTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Vision
{  
    using System;
    using System.IO;
    using System.Windows.Media.Imaging;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.GroundTruthExtraction;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Planar pose estimation related tests.
    /// The test is not able to resolve native dll dependencies to copy when run individually, so they are copied explicitly
    /// </summary>
    [TestClass]
    [DeploymentItem(@"Microsoft.Robotics.Vision.Native.dll")]
    [DeploymentItem(@"Microsoft.Robotics.Vision.Interop.dll")]
    [DeploymentItem(@"Data\GroundTruthExtraction\calib_board.txt", @"Data\GroundTruthExtraction")]
    public class PlanarPoseEstimationTest
    {
        /// <summary>
        /// Planar Pose Reference Results for each file.
        /// X offset (meters), Y offset (meters), Z offset (meters), Angle X (radians), Angle Y (Radians), AngleZ (Radians)
        /// </summary>
        private static readonly double[,] PlanarPoseReferenceResults = 
        {
            { 0.269120600347658, 0.161135244342377, 2.11556658461009, 0.00397146090265256, -0.0362032405524052, 0.00575810553143619 }, 
            { 0.26869918574943, 0.00231244742965291, 2.11225358693026, 0.00705595892635979, -0.0406412117229351, 0.0059562317200616 }, 
            { 0.266765223920652, -0.156478662997928, 2.11292613207422, 0.0151892920137603, -0.0358634930679055, 0.00661956225815369 }, 
            { -0.0583251501654562, 0.162613941770271, 2.10987138361882, -0.00234020653304766, -0.0362799307519775, 0.00826176920020628 }, 
            { -0.0586625630017354, 0.0040607106071761, 2.10805842697888, -0.000303598179650426, -0.0275959961573257, 0.00327136395752549 }, 
            { -0.0653554969498513, -0.160066149377441, 2.10513931379655, 0.0336880468747691, -0.0398498264203031, 0.00499655343064838 }, 
            { -0.389794647200533, 0.168160091127336, 2.10087809023054, -0.0139405045250446, -0.0118029790185819, -0.00377743791727557 }, 
            { -0.381188953668631, 0.0137075127905321, 2.10038230095744, -0.00663369997942841, -0.00428709149969967, 0.00433465455166967 }, 
            { -0.381851620659117, -0.144143667956236, 2.09831861841164, 0.00602931593100286, -0.00632531134671756, 0.00598253376767179 }, 
            { 0.0184994955019283, 0.0551247747224668, 2.10852355921491, -0.0026141928135746, -0.00862158278382957, -2.08207871970123 }, 
            { 0.0407717262803299, 0.0366598523294718, 2.10871046500691, -0.00199244617165252, -0.0155471461991094, -2.60736674248089 }, 
            { 0.257755211438151, -0.0671353595654299, 2.11163617025613, 0.00637276725830342, -0.0407972490255863, -0.252270348419268 }, 
            { 0.0500524026333575, 0.0100163634217163, 2.10738405011658, -0.000524305957640249, -0.0269907854047735, -3.12861833893276 }, 
            { 0.0445632319202433, -0.0178933897253847, 2.10773737723762, 0.000582507190181525, -0.0148024602745357, 2.62919317608259 }, 
            { 0.0237532683025594, -0.0372061410446964, 2.10682301816568, 0.00847110101717656, -0.0151065185593058, 2.10176787930457 }, 
            { -0.00412028010477349, -0.0441697981170926, 2.10755911617439, 0.00724289251600641, -0.00856872369064691, 1.57766304174229 }, 
            { -0.0314956299258988, -0.0357671978245767, 2.10748179479969, 0.00232327840515411, -0.0306768051648336, 1.05621085445876 }, 
            { -0.185197120528638, -0.317531348347927, 2.09835082113556, 0.00808508415710472, -0.00599925821462918, 1.05500387960021 }, 
            { 0.308068351462184, 0.00553123358673658, 2.11360506441764, 0.00305450690760941, -0.0383675169527134, -0.513711189890847 }, 
            { -0.0516810925767282, -0.018598478337455, 2.10820140126056, 0.00907233027259538, -0.0233674671526343, 0.533624150325606 }, 
            { -0.250140627670394, -0.318472988302183, 2.09626198878832, 0.00804975464340562, -0.00178165168903164, 0.533583199164219 }, 
            { -0.0606680610625949, 0.00775119184101048, 2.10807213482607, 0.00327800352793788, -0.025911134531827, 0.00607263760462422 }, 
            { -0.0594568824977674, -0.153693453213932, 2.10712448185062, 0.0311725938964705, -0.00188524803700415, 0.00872799036767368 }, 
            { -0.0456536960481223, 0.0430444698655938, 2.10877817240777, -0.00196541811561515, -0.0247380195794369, -0.773888561469495 }, 
            { 0.102445992583092, 0.131698407382832, 2.11009950677673, 0.00166198857504221, -0.0276323990316817, -1.03686558372732 }, 
            { -0.00848648380131332, 0.060817448236646, 2.10807109314145, 0.00761388897999011, -0.0056096544885372, -1.56276927351895 }, 
        };

        /// <summary>
        /// Reference Camera Intrinsic
        /// </summary>
        private static double[] colorIntrinsicParams;

        /// <summary>
        /// Reference Camera Distortion Parameters
        /// </summary>
        private static double[] colorRDistortionParams;

        /// <summary>
        /// Initialize the reference ground truth results.
        /// </summary>
        /// <param name="tc">Text context</param>
        [ClassInitialize]
        public static void InitializeGroundTruth(TestContext tc)
        { 
            colorIntrinsicParams = new double[]
            {
                989.704,
                0,
                988.807,
                657.014,
                344.698
            };
            colorRDistortionParams = new double[]
        {
                0.0267711,
                -0.0654381,
                -0.000869768,
                -0.00127955,
                0
            };
        }

        /// <summary>
        /// Tests camera pose estimation by performing it on a reference image and comparing the results.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PlanarPoseEstimationUsingReferenceImage()
        {
            CheckCalibrationBoardFileExists();

            var files = Directory.EnumerateFiles(@"\\robofile\baker\Shared\Users\roboim\UnitTestData\GroundTruthExtraction", "*.png");
            int filesIndex = 0;
            foreach (var file in files)
                                                {
                int upperIndex0 = PlanarPoseReferenceResults.GetUpperBound(0);                
                if (filesIndex > upperIndex0)
            {
                    Assert.Fail("Ground truth reference files directory has more files than expected");
                return;
            }

                VerifyGroundTruthForReferenceFile(file, filesIndex);    
                ++filesIndex;                            
            }
        }

        /// <summary>
        /// Ensure that if only the checkerboard is visible and less than 6 corner points are visible, the MSR codebase returns false
        /// and does not crash. (Bug 1538)
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PlanarPoseEstimationUsingCheckerboadWithLessThan6Points()
        {
            CheckCalibrationBoardFileExists();
            
            string filename = @"\\robofile\baker\Shared\Users\roboim\UnitTestData\GroundTruthExtraction\CheckerBoardWithZeroPoints\CheckerBoardWithZeroPointsGrayScale.png";
           ImageFrameGray8 image = GetGrayScaleImageForBitmapFile(filename);
            
            double[] translation = new double[3];
            double[] rotation = new double[3];
             unsafe
             {
                 fixed (double* colorIntrinsicParamsPtr = colorIntrinsicParams)
                     fixed (double* colorRDistortionParamsPtr = colorRDistortionParams)
                         fixed (double* translationPtr = translation)
                             fixed (double* rotationPtr = rotation)
                             {
                                     ManagedPlanarPoseEstimator planarPoseEstimator = new ManagedPlanarPoseEstimator(
                                         colorIntrinsicParamsPtr, 
                                         colorIntrinsicParams.Length,
                                         colorRDistortionParamsPtr,
                                         colorRDistortionParams.Length);
                                     
                                     bool foundCheckerBoard = planarPoseEstimator.EstimatePose(
                                         image,
                                         translationPtr,
                                         (uint)translation.Length,
                                         rotationPtr,
                                         (uint)rotation.Length);

                                     Assert.IsFalse(foundCheckerBoard, "Checkboard board with less than 6 points should return false");
                            }
             }
        }

        /// <summary>
        /// Returns a gray scale image for the give bitmap file.
        /// </summary>
        /// <param name="filename">Bitmap file path</param>
        /// <returns>Gray scale image</returns>
        private static ImageFrameGray8 GetGrayScaleImageForBitmapFile(string filename)
        {
            BitmapDecoder uriBitmap = BitmapDecoder.Create(
            new Uri(filename, UriKind.Relative),
            BitmapCreateOptions.PreservePixelFormat,
            BitmapCacheOption.Default);

            BitmapFrame bitmapFrame = uriBitmap.Frames[0];
            ImageFrameGray8 image = ImageOperations2DInterop.CreateImageFrameGray8FromBitmapFrame(bitmapFrame);
            return image;
        }

        /// <summary>
        /// Verify the ground truth result for the given reference file
        /// </summary>
        /// <param name="refFilename">Reference file Name</param>
        /// <param name="refFileIndex">Reference file index to get the corresponding results</param>
        private static void VerifyGroundTruthForReferenceFile(string refFilename, int refFileIndex)
        {
            ImageFrameGray8 image = GetGrayScaleImageForBitmapFile(refFilename);
            
            double[] translation = new double[3];
            double[] rotation = new double[3];
             unsafe
             {
                 fixed (double* colorIntrinsicParamsPtr = colorIntrinsicParams)
                     fixed (double* colorRDistortionParamsPtr = colorRDistortionParams)
                         fixed (double* translationPtr = translation)
                             fixed (double* rotationPtr = rotation)
                             {
                                     ManagedPlanarPoseEstimator planarPoseEstimator = new ManagedPlanarPoseEstimator(
                                         colorIntrinsicParamsPtr, 
                                         colorIntrinsicParams.Length,
                                         colorRDistortionParamsPtr,
                                         colorRDistortionParams.Length);
                                     
                                     bool foundCheckerBoard = planarPoseEstimator.EstimatePose(
                                         image,
                                         translationPtr,
                                         (uint)translation.Length,
                                         rotationPtr,
                                         (uint)rotation.Length);
                                    TraceOut.Info(TraceContexts.Test, "{{ {0}, {1}, {2}, {3}, {4}, {5} }}, ", translation[0], translation[1], translation[2], rotation[0], rotation[1], rotation[2]);
                                    Assert.IsTrue(foundCheckerBoard, "Could not detect checkerboard for image {0}", refFilename);
                                    const double PlanarPoseErrorEpsilon = 1e-4;
                                    Assert.AreEqual(PlanarPoseReferenceResults[refFileIndex, 0], translation[0], PlanarPoseErrorEpsilon, "Translation 0 doesn't match expected value");
                                    Assert.AreEqual(PlanarPoseReferenceResults[refFileIndex, 1], translation[1], PlanarPoseErrorEpsilon, "Translation 1 doesn't match expected value");
                                    Assert.AreEqual(PlanarPoseReferenceResults[refFileIndex, 2], translation[2], PlanarPoseErrorEpsilon, "Translation 2 doesn't match expected value");
                                    Assert.AreEqual(PlanarPoseReferenceResults[refFileIndex, 3], rotation[0], PlanarPoseErrorEpsilon, "Rotation 0 doesn't match expected value");
                                    Assert.AreEqual(PlanarPoseReferenceResults[refFileIndex, 4], rotation[1], PlanarPoseErrorEpsilon, "Rotation 1 doesn't match expected value");
                                    Assert.AreEqual(PlanarPoseReferenceResults[refFileIndex, 5], rotation[2], PlanarPoseErrorEpsilon, "Rotation 2 doesn't match expected value");
                            }
            }
        }

        /// <summary>
        /// Ensure that the calibration board resource exists.
        /// </summary>
        private static void CheckCalibrationBoardFileExists()
        {
            string calibBoardFileName = @"Data\GroundTruthExtraction\calib_board.txt";

            if (!File.Exists(calibBoardFileName))
            {
                Assert.Fail(@"Test setup error Data\GroundTruthExtraction\calib_board.txt not found");
            }
        }
    }
}
